﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using MathLib;
using TrackingSim.GameSystem;
using TrackingSim.Filtering.Tracks;
using TrackingSim.Filtering.Kinematics;
using TrackingSim.Filtering.Measurements;

namespace TrackingSim.Filtering.Updating
{
    class R6_with_Razel
    {
        public static void extrap_covariance(Track_R6 track, double dt, MathLib.Matrix Q)
        {
            MathLib.Matrix phi = Kalman.phi_pos_vel_6state(dt);

            track.KS.Cov = phi * track.KS.Cov * phi.tp() + Q;
        }

        public static void extrap_state(Track_R6 track, double dt)
        {
            MathLib.Matrix phi = Kalman.phi_pos_vel_6state(dt);

            State_R6 state = (State_R6)track.KS;
            state.State = phi * state.State;

            double accel = 0.0;

            state.Xdot = state.Xdot + accel * dt;
            state.Ydot = state.Ydot + accel * dt;
            state.Zdot = state.Zdot + accel * dt;
        }

        public static Track_R6 init_track(Player p, Razel meas)
        {
            Track_R6 new_track = new Track_R6(meas.Time);

            State_R6 state = (State_R6)new_track.KS;

            MathLib.Vector delta_body = meas.R * meas.UnitLOS;
            MathLib.Vector delta_abs = Coords.abs_to_body(p).tp() * delta_body;

            state.State.X = p.Pos.X + delta_abs.X;
            state.State.Y = p.Pos.Y + delta_abs.Y;
            state.State.Z = p.Pos.Z + delta_abs.Z;

            state.Cov.m[0, 0] = 25.0;
            state.Cov.m[1, 1] = 25.0;
            state.Cov.m[2, 2] = 25.0;
            state.Cov.m[3, 3] = 9.0;
            state.Cov.m[4, 4] = 9.0;
            state.Cov.m[5, 5] = 9.0;

            return new_track;
        }

        public static void update(Track_R6 track, Razel meas, Player host)
        {
            State_R6 state = (State_R6)track.KS;

            double dt = meas.Time - track.Time;
            track.Time = meas.Time;

            MathLib.Matrix Q = new MathLib.Matrix(6, 6);
            Q.m[0, 0] = 100.0 * dt;
            Q.m[1, 1] = 100.0 * dt;
            Q.m[2, 2] = 100.0 * dt;

            extrap_covariance(track, dt, Q);
            extrap_state(track, dt);

            MathLib.Matrix z = get_Z(meas);
            MathLib.Matrix z_est = get_Z(track, host);
            MathLib.Matrix innovation = z - z_est;
            MathLib.Matrix R = get_R(meas);
            MathLib.Matrix H = get_H(track, host);

            MathLib.Vector state_vector = new MathLib.Vector(6);
            MathLib.Matrix cov = new MathLib.Matrix(6, 6);
            state_vector.set_from(state.State);
            cov.set_from_matrix(state.Cov);

            state_vector = Coords.abs_to_body_6x6(host) * state_vector;
            cov = Coords.abs_to_body_6x6(host) * cov * Coords.abs_to_body_6x6(host).tp();

            Kalman.update_track_r6(ref state_vector, ref cov, innovation, H, R);

            state_vector = Coords.abs_to_body_6x6(host).tp() * state_vector;
            cov = Coords.abs_to_body_6x6(host).tp() * cov * Coords.abs_to_body_6x6(host);

            state.State.set_from(state_vector);
            state.Cov.set_from_matrix(cov);
        }

        private static double sqr(double x)
        {
            return x * x;
        }

        public static MathLib.Matrix get_Z(Razel meas)
        {
            MathLib.Matrix Z = new Matrix(3, 1);

            Z.m[0, 0] = meas.R;
            Z.m[1, 0] = meas.Az;
            Z.m[2, 0] = meas.El;

            return Z;
        }

        public static Matrix get_Z(Track_R6 track, Player p)
        {
            State_R6 state = (State_R6)track.KS;

            Matrix Z = new Matrix(3, 1);
            Vector pos_body = Coords.abs_to_body(p) * state.Pos;

            Z.m[0, 0] = pos_body.mag();
            Z.m[1, 0] = Math.Atan2(pos_body.Y, pos_body.X);
            Z.m[2, 0] = Math.Atan2(pos_body.Z, Math.Sqrt(sqr(pos_body.X) + sqr(pos_body.Y)));

            return Z;
        }

        public static MathLib.Matrix get_H(Track_R6 track, Player p)
        {
            State_R6 state = (State_R6)track.KS;
            MathLib.Matrix H = new MathLib.Matrix(3, 6);

            MathLib.Vector pos_body = Coords.abs_to_body(p) * state.Pos;

            double x = pos_body.X;
            double y = pos_body.Y;
            double z = pos_body.Z;
            double pos_mag = pos_body.mag();

            H.m[0, 0] = x / pos_mag;
            H.m[0, 1] = y / pos_mag;
            H.m[0, 2] = z / pos_mag;

            double y2_p_x2 = sqr(y) + sqr(x);

            H.m[1, 0] = -y / y2_p_x2;
            H.m[1, 1] = x / y2_p_x2;
            H.m[1, 2] = 0.0;

            double z2_p_y2_p_x2 = sqr(z) + y2_p_x2;
            double sqrt_y2_p_x2 = Math.Sqrt(y2_p_x2);

            H.m[2, 0] = (-x * z) / (sqrt_y2_p_x2 * z2_p_y2_p_x2);
            H.m[2, 1] = (-y * z) / (sqrt_y2_p_x2 * z2_p_y2_p_x2);
            H.m[2, 2] = sqrt_y2_p_x2 / z2_p_y2_p_x2;

            return H;
        }

        public static MathLib.Matrix get_R(Razel meas)
        {
            MathLib.Matrix R = new Matrix(3, 3);

            R.m[0, 0] = sqr(meas.R_random_std);
            R.m[1, 1] = sqr(meas.Az_random_std);
            R.m[2, 2] = sqr(meas.El_random_std);

            return R;
        }
    }
}
